#include <math.h>
//#include <string.hpp>
#include <stdio.h>

#include <sstream>


#include <iostream>
#include <string>
#include <cstring>



#include <webots/Robot.hpp>


#include <ros/ros.h>
#include <sensor_msgs/Image.h>

#include <CR1/RangeCameraParam.h>

using namespace std;
using namespace webots;


class RWRangeCamera
{
public:
  RWRangeCamera();
  void init(webots::Robot*,ros::NodeHandle*,string);
  ~RWRangeCamera();

  void iterate();
private:
  double time_step;
  double range_width, range_height,range_min_range,range_max_range;
  Camera* RANGE;
  const float *lms_values;
  ros::Publisher pub;
  ros::Publisher param_pub;
  ros::NodeHandle* n;

  sensor_msgs::Image RangeImage;
  CR1::RangeCameraParam param;

  int param_pub_cont;
};
